/*
 * Copyright (C) 2023 ab_skywalker@163.com All Rights Reserved.
 *
 * SPDX-License-Identifier: MIT
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of
 * this software and associated documentation files (the "Software"), to deal in
 * the Software without restriction, including without limitation the rights to
 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
 * the Software, and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
 * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
 * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
 * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 *
 */
#include "foc_park_clark.h"
#include "foc_util.h"
#include <stdint.h>

#define DIV_BY_SQRT3(x) (((x) * 1000) / 1732)
#define sqrt3 1732

uint8_t CurSector = 0;

void clarkTransform(DIM_THREE_S origincur, DIM_TWO_S *cur)
{
    (*cur).alpha = origincur.a;
    (*cur).beta = DIV_BY_SQRT3(origincur.b - origincur.c);
}

void parkTransform(DIM_TWO_S origincur, int32_t pahse, DIM_ROTATE_S *cur)
{
    int32_t c = arm_cos_f32(pahse);
    int32_t s = arm_sin_f32(pahse);
    (*cur).d = (c*origincur.alpha + s*origincur.beta)/TRIGONO_NORM;
    (*cur).q = (c*origincur.beta - s*origincur.alpha)/TRIGONO_NORM;
}

void reserveParkTransform(DIM_ROTATE_S originvol, int32_t pahse, DIM_TWO_S *vol)
{
    int32_t c = arm_cos_f32(pahse);
    int32_t s = arm_sin_f32(pahse);
    (*vol).alpha = (c*originvol.d - s*originvol.q)/TRIGONO_NORM;
    (*vol).beta  = (c*originvol.q + s*originvol.d)/TRIGONO_NORM;
}

void svpwmTransform(DIM_TWO_S vol, DIM_THREE_S *duty)
{
    uint32_t timelimit = sqrt3 / 2;
    uint32_t t0 = 0, t1 = 0, t2 = 0;
    uint8_t sectiontable[] = {0, 2, 6, 1, 4, 3, 5, 0};
    int32_t U1 = vol.beta;
    int32_t U2 = (sqrt3 * vol.alpha - 1000 * vol.beta) / 2000;
    int32_t U3 = (-1 * sqrt3 * vol.alpha - 1000 * vol.beta) / 2000;
    int16_t ta = 0, tb = 0, tc = 0;
    uint8_t a = U1 > 0 ? 1 : 0;
    uint8_t b = U2 > 0 ? 1 : 0;
    uint8_t c = U3 > 0 ? 1 : 0;
    uint8_t n = 4 * c + 2 * b + a;
    CurSector = sectiontable[n];
    switch(sectiontable[n]) {
        case 1:
            t1 = U2;//4
            t2 = U1;//6
            break;
        case 2:
            t1 = -U3;//6
            t2 = -U2;//2
            break;
        case 3:
            t1 = U1;//2
            t2 = U3;//3
            break;
        case 4:
            t1 = -U2;//3
            t2 = -U1;//1
            break;
        case 5:
            t1 = U3;//1
            t2 = U2;//5
            break;
        case 6:
            t1 = -U1;//5
            t2 = -U3;//4
            break;
        default:
            break;
    }
    //最大到六边形的内切圆，
    if(t1 + t2 > timelimit) {
        uint32_t temp = t1 + t2;
        t1 = t1 * (timelimit) / temp;
        t2 = t2 * (timelimit) / temp;
    }

    t0 = (timelimit - t1 - t2) >> 1;

    switch(sectiontable[n]) {
        case 1:
            ta = t0 >> 1;
            tb = (t0 + t1) >> 1;
            tc = (t0 + t1 + t2) >> 1;
            break;
        case 2:
            ta = (t0 + t2) >> 1;
            tb = t0 >> 1;
            tc = (t0 + t1 + t2) >> 1;
            break;
        case 3:
            ta = (t0 + t1 + t2) >> 1;
            tb = t0 >> 1;
            tc = (t0 + t1) >> 1;
            break;
        case 4:
            ta = (t0 + t1 + t2) >> 1;
            tb = (t0 + t2) >> 1;
            tc = t0 >> 1;
            break;
        case 5:
            ta = (t0 + t1) >> 1;
            tb = (t0 + t1 + t2) >> 1;
            tc = t0 >> 1;
            break;
        case 6:
            ta = t0 >> 1;
            tb = (t0 + t1 + t2) >> 1;
            tc = (t0 + t2) >> 1;
            break;
        default:
            break;
    }
    duty->a = ta;
    duty->b = tb;
    duty->c = tc;
}

uint8_t foc_getSector(uint16_t phase)
{
    return phase * 6 / 6280;
}
